
/****************************************************************************
*							Adapters Class header							*
*    				Copyright (C) 2012  J.R. Ruiz-Sarmiento					*
*																			*
*    This program is free software: you can redistribute it and/or modify	*
*    it under the terms of the GNU General Public License as published by	*
*    the Free Software Foundation, either version 3 of the License, or		*
*    (at your option) any later version.									*
*																			*
*    This program is distributed in the hope that it will be useful,		*
*    but WITHOUT ANY WARRANTY; without even the implied warranty of			*
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the			*
*    GNU General Public License for more details.							*
*																			*
*    You should have received a copy of the GNU General Public License		*
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.	*
*																			*
****************************************************************************/

#ifndef _POINTSCLOUDADAPTER_
#define _POINTSCLOUDADAPTER_

#include <mrpt/slam/CObservation3DRangeScan.h>

#include <mrpt/slam/PCL_adapters.h> 

#include "Models.h"

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>

#include <tinyxml.h>

namespace SORT
{
namespace Adapters
{

	class CDataAdapters
	{
	public:
		
		static void RangeScan3DToPointCloud( mrpt::slam::CObservation3DRangeScanPtr &newObs, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud )
		{	
			newObs->project3DPointsFromDepthImageInto( *cloud, true );

			Eigen::Matrix4f transMat;

			transMat(0,0)=0;    transMat(0,1)=-1;     transMat(0,2)=0;    transMat(0,3)=0;
			transMat(1,0)=0;    transMat(1,1)=0;      transMat(1,2)=-1;   transMat(1,3)=0;
			transMat(2,0)=1;    transMat(2,1)=0;      transMat(2,2)=0;    transMat(2,3)=0;
			transMat(3,0)=0;    transMat(3,1)=0;      transMat(3,2)=0;    transMat(3,3)=1;

			pcl::transformPointCloud( *cloud, *cloud, transMat );	

			cloud->width = 640;
			cloud->height = 480;
		}

		static void RangeScan3DToPointCloud( mrpt::slam::CObservation3DRangeScanPtr &newObs, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, std::vector<int> &validPoints )
		{
			validPoints.clear();

			newObs->project3DPointsFromDepthImageInto( *cloud, true );

			Eigen::Matrix4f transMat;

			transMat(0,0)=0;    transMat(0,1)=-1;     transMat(0,2)=0;    transMat(0,3)=0;
			transMat(1,0)=0;    transMat(1,1)=0;      transMat(1,2)=-1;   transMat(1,3)=0;
			transMat(2,0)=1;    transMat(2,1)=0;      transMat(2,2)=0;    transMat(2,3)=0;
			transMat(3,0)=0;    transMat(3,1)=0;      transMat(3,2)=0;    transMat(3,3)=1;

			pcl::transformPointCloud( *cloud, *cloud, transMat );	

			cloud->width = 640;
			cloud->height = 480;

			size_t N = cloud->size();

			for ( size_t i = 0; i < N; i++ )
			{
				if ( cloud->points[i].z > 0.4 )
					validPoints.push_back( i );
			}
		}

		static bool PreloadedPbMapToCPlanes( TiXmlDocument &xmlDoc, mrpt::aligned_containers<SORT::ObjectRecognition::CPlane>::vector_t &v_planes );

		static bool PreloadedClassificationResultsToCPlanes ( TiXmlDocument &xmlDoc, mrpt::aligned_containers<SORT::ObjectRecognition::CPlane>::vector_t &v_planes );

		static bool OnlineClassificationResultsToCPlanes ( std::string res, mrpt::aligned_containers<SORT::ObjectRecognition::CPlane>::vector_t &v_planes );

		static void getFeatures( std::string &detection, std::map<std::string, std::string> &features );

	private:

		template<typename T> static void tokenizeNumericalString( const string &str, vector<T> &values, const string &separator )
		{
			values.clear();

			vector<string> tokens;
			boost::split(tokens, str, boost::is_any_of(separator));

			string type( typeid(T).name() );

			size_t N_tokens = tokens.size();

			values.resize( N_tokens );
	
			for ( size_t i = 0; i < N_tokens; i++ )
			{
				if ( (type == "float" ) || ( type == "double" ) )
					values[i] =  atof( tokens[i].c_str() );

				else if ( type == "int" )
					values[i] = atoi( tokens[i].c_str() );
			}
		}

		static void tokenizeString( const string &str, vector<string> &values, const string &separator )
		{
			values.clear();
			boost::split(values, str, boost::is_any_of(separator));
		}

	};
}
}

#endif
